#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "Vector3.h"

/* compile with -lpthread and -llo */

#include <pthread.h>
#include "lo/lo.h"

#define UPDATE_PATH "/pfs/listener/0001/updateAxes"
#define INFINITY_COMMAND "/infinity/target"
#define PLC_NUMBER 7
#define LISTEN_PORT "25669"
#define LOCAL_PORT "25670"
#define LOOPS 1000
#define NUM_AXES 8

void * server_thread( void * arg );
void * position_reporter( void * arg);
void * target_reporter( void * arg);

int upperbound = 0;
int lowerbound = 0;
int rightbound = 0;
int leftbound = 0;
int forwardbound = 0;
int backwardbound = 0;

int emergencystop = 0;
int count = 0;
int updatecount = 0;
int targetcount = 0;
int done = 0;

int speed = 0;
int aspeed = 0;
int bspeed = 0;
int cspeed = 0;
int dspeed = 0;

lo_address t;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int axis_position[NUM_AXES] = { 1 };
int axis_status[NUM_AXES] = { 0 };
int axis_error[NUM_AXES] = { 0 };
int axis_number[NUM_AXES] = { 0 };
int axis_load[NUM_AXES] = { 0 };

int axis_target[3] = { 0 };

lo_address local_broadcast;

Vector3 wincha;
Vector3 winchb;
Vector3 winchc;
Vector3 winchd;

int amin = 25943;
int bmin = 25625;
int cmin = 25758;
int dmin = 26340;

int amax = 36043;
int bmax = 36772;
int cmax = 36922;
int dmax = 37698;

int aoffset = 3933;
int boffset = 3384;
int coffset = 2684;
int doffset = 2658;

int sn = 40000;

Vector3 position;
Vector3 starting;
Vector3 target;

Vector3 A,B,C,D;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int main() {
	pthread_t tid[3];
	int rc;

	t = lo_address_new("192.168.2.57", "25666");

	local_broadcast = lo_address_new("192.168.2.89", LOCAL_PORT);

	wincha.x = 0;
	wincha.y = 0;
	wincha.z = 0;

	winchb.x = 9753;
	winchb.y = 0;
	winchb.z = 548;

	winchc.x = 9753;
	winchc.y = 0;
	winchc.z = 11643;

	winchd.x = 0;
	winchd.y = 0;
	winchd.z = 12192;

	position.x = 5000;
	position.y = 5000;
	position.z = 6000;

	target.x = 0;
	target.y = 0;
	target.z = 0;

	starting.x = 5000;
	starting.y = 5000;
	starting.z = 6000;

	printf("Creating threads\n");
	rc = pthread_create( &tid[0], NULL, server_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[1], NULL, position_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[2], NULL, target_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	printf("Threads created\n");

	pthread_join( tid[0], NULL );
	printf( "Terminating.\n" );
	pthread_mutex_destroy(&mutex);

	return 0;
}

void loadstate(int state){
	//sn++;
	if(sn > 65535)
		sn = 0;
	lo_message message = lo_message_new();
	lo_message_add(message, "ii", sn, state);
	lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
	lo_bundle_add_message(bundle, "/pfs/ur/0006/loadState", message); 
	lo_send_bundle(t, bundle);
	lo_bundle_free(bundle);
	lo_message_free(message);
}

void startplayback(char* path){
	//sn++;
	if(sn > 65535)
		sn = 0;
	lo_message message = lo_message_new();
	lo_message_add(message, "i", sn);
	lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
	lo_bundle_add_message(bundle, path, message); 
	lo_send_bundle(t, bundle);
	lo_bundle_free(bundle);
	lo_message_free(message);
}

void sendtoposition(char* path, int axis, int position){
	//sn++;
	if(sn > 65535)
		sn = 0;
	lo_message message = lo_message_new();
	lo_message_add(message, "iii", sn, axis, position);
	lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
	lo_bundle_add_message(bundle, path, message); 
	lo_send_bundle(t, bundle);
	lo_bundle_free(bundle);
	lo_message_free(message);
}

void setspeed(char* path, int speed){
	//sn++;
	if(sn > 65535)
		sn = 0;
	lo_message message = lo_message_new();
	lo_message_add(message, "ii", sn, speed);
	lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
	lo_bundle_add_message(bundle, path, message); 
	lo_send_bundle(t, bundle);
	lo_bundle_free(bundle);
	lo_message_free(message);
}

void error(int num, const char *msg, const char *path)
{
	printf("liblo server error %d in path %s: %s\n", num, path, msg);
	fflush(stdout);
}

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	int PLC = argv[0]->i;
	if(PLC != PLC_NUMBER)
		return 1;
	pthread_mutex_lock(&mutex);
	int i;
	for(i = 0; i < NUM_AXES; i++){
		axis_number[i] = argv[(i*5)+2]->i;
		axis_position[i] = argv[(i*5)+3]->i;
		axis_load[i] = argv[(i*5)+4]->i;
		axis_status[i] = argv[(i*5)+5]->i;
		axis_error[i] = argv[(i*5)+6]->i;
	}

	updatecount++;
	pthread_mutex_unlock(&mutex);
	fflush(stdout);

	return 1;
}

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	target.x = argv[0]->f;
	target.y = argv[1]->f;
	target.z = argv[2]->f;
	/*if(target.z > 10000)
	target.z = 10000;
	if(target.z < 2650)
	target.z = 2650;
	if(target.y > 5500)
	target.y = 5500;
	if(target.y < 2650)
	target.y = 2650;
	if(target.x < 2613)
	target.x = 2613;
	if(target.x > 7900)
	target.x = 7900;*/

	speed = argv[3]->i;
	targetcount++;

	int a = (int)vecLength(vecSub(wincha, target));
	int b = (int)vecLength(vecSub(winchb, target));
	int c = (int)vecLength(vecSub(winchc, target));
	int d = (int)vecLength(vecSub(winchd, target));
	a = a - aoffset;
	b = b - boffset;
	c = c - coffset;
	d = d - doffset;
	a = amax - a;
	b = bmax - b;
	c = cmax - c;
	d = dmax - d;
	axis_target[0] = a;
	axis_target[1] = b;
	axis_target[2] = c;
	axis_target[3] = d;
	pthread_mutex_unlock(&mutex);
	return 1;  
}

void * position_reporter(void * arg){
	int i;
	while(1){
		pthread_mutex_lock(&mutex);
		position.x = starting.x;
		position.y = starting.y;
		position.z = starting.z;
		for( i = 0; i < LOOPS; i++){
			A = vecSub(position, wincha);
			B = vecSub(position, winchb);
			C = vecSub(position, winchc);
			D = vecSub(position, winchd);
			A = vecAdd(vecMult(A,((double)(amax-axis_position[1]+aoffset))/vecLength(A)), wincha);
			B = vecAdd(vecMult(B,((double)(bmax-axis_position[2]+boffset))/vecLength(B)), winchb);
			C = vecAdd(vecMult(C,((double)(cmax-axis_position[5]+coffset))/vecLength(C)), winchc);
			D = vecAdd(vecMult(D,((double)(dmax-axis_position[6]+doffset))/vecLength(D)), winchd);
			position = vecAdd(A,B);
			position = vecAdd(position,C);
			position = vecAdd(position,D);
			position = vecDiv(position,4);
		}
		count++;
		pthread_mutex_unlock(&mutex);
		if (lo_send(local_broadcast, "/infinity/update", "fff", position.x, position.y, position.z) == -1) {
			printf("OSC error %d: %s\n", lo_address_errno(local_broadcast), lo_address_errstr(local_broadcast));
		}
		usleep(50000);
	}
}

void * target_reporter(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    if(axis_target[0] < amax && axis_target[0] > amin && axis_target[1] < bmax && axis_target[1] > bmin && axis_target[2] < cmax && axis_target[2] > cmin && axis_target[3] < dmax && axis_target[3] > dmin){
      double high = 0;
      int a = abs(axis_target[0]-axis_position[1]);
      int b = abs(axis_target[1]-axis_position[2]);
      int c = abs(axis_target[2]-axis_position[5]);
      int d = abs(axis_target[3]-axis_position[6]);
      if(a >= high)
        high = a;
      if(b > high)
        high = b;
      if(c > high)
        high = c;
      if(d > high)
        high = d;
      if(high > 0){
        aspeed = a/high*100;
        bspeed = b/high*100;
        cspeed = c/high*100;
        dspeed = d/high*100;
      }
      if(vecLength(vecSub(target,position)) > 50){
        setspeed("/pfs/ur/0006/pb/1/setSpeed", (int)ceil(speed * ((double)aspeed/100.0)));
        setspeed("/pfs/ur/0006/pb/2/setSpeed", (int)ceil(speed * ((double)bspeed/100.0)));
        setspeed("/pfs/ur/0006/pb/3/setSpeed", (int)ceil(speed * ((double)cspeed/100.0)));
        setspeed("/pfs/ur/0006/pb/4/setSpeed", (int)ceil(speed * ((double)dspeed/100.0)));
        sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_target[0]);
        sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, axis_target[1]);
        sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, axis_target[2]);
        sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, axis_target[3]);
      }
      else{
        setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/3/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/4/setSpeed", 0);
      }
    }
    else{
      printf("OUT OF RANGE\n");
    }
    pthread_mutex_unlock(&mutex);
    usleep(200000);
  }
}

void * server_thread( void * arg ) {
	printf("In thread\n");
	/* start a new server on port LISTEN_PORT */
	lo_server_thread st = lo_server_thread_new(LISTEN_PORT, error);

	/* add function to catch UPDATE_PATH OSC commands */
	lo_server_thread_add_method(st, UPDATE_PATH, NULL, update_handler, NULL);

	lo_server_thread_add_method(st, INFINITY_COMMAND, "fffi", target_handler, NULL);

	lo_server_thread_add_method(st, "/stop", NULL, emergency_handler, NULL);

	lo_server_thread_add_method(st, "/state", "i", state_handler, NULL);

	lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

	lo_server_thread_start(st);

	while (!done) {
		//Polling Loop
		usleep(1000000);
		pthread_mutex_lock(&mutex);
    int i;
    if(updatecount == 0){
      printf("NO UPDATES!\n");
    }
    else{
      if(target.x == 0 && target.y == 0 && target.z == 0){
        target.x = position.x;
        target.y = position.y;
        target.z = position.z;

        int a = (int)vecLength(vecSub(wincha, target));
        int b = (int)vecLength(vecSub(winchb, target));
        int c = (int)vecLength(vecSub(winchc, target));
        int d = (int)vecLength(vecSub(winchd, target));
        a = a - aoffset;
        b = b - boffset;
        c = c - coffset;
        d = d - doffset;
        a = amax - a;
        b = bmax - b;
	c = cmax - c;
	d = dmax - d;

        axis_target[0] = a;
        axis_target[1] = b;
        axis_target[2] = c;
        axis_target[3] = d;
      }
      for(i = 0; i < NUM_AXES; i++){
        printf("Axis: %d Pos: %d Load: %d Status: %d Error: %d\n", axis_number[i], axis_position[i], axis_load[i], axis_status[i], axis_error[i]);
      }
      printf("Position X: %f Y: %f Z: %f count: %d\n", position.x, position.y, position.z, count);
      printf("Target X: %f Y: %f Z: %f count: %d\n", target.x, target.y, target.z, targetcount);
      printf("A length: %d B length: %d C length: %d D length: %d\n", (amax-axis_position[1]+aoffset), (bmax-axis_position[2]+boffset), (cmax-axis_position[5]+coffset), (dmax-axis_position[6]+doffset));
      printf("PREDICTED: A length: %d B length %d C length %d D length %d\n",
        (int)vecLength(vecSub(wincha, position)),
        (int)vecLength(vecSub(winchb, position)),
        (int)vecLength(vecSub(winchc, position)),
        (int)vecLength(vecSub(winchd, position)));
      printf("Target: A length: %d B length %d C length %d D length %d\n", (amax-axis_target[0]+aoffset), (bmax-axis_target[1]+boffset), (cmax-axis_target[2]+coffset), (dmax-axis_target[3]+doffset));
      printf("Aspeed: %d Bspeed: %d Cspeed: %d Dspeed: %d\n",(int)ceil(speed * ((double)aspeed/100.0)),(int)ceil(speed * ((double)bspeed/100.0)), (int)ceil(speed * ((double)cspeed/100.0)), (int)ceil(speed * ((double)dspeed/100.0)));
      updatecount = 0;
      targetcount = 0;
      count = 0;
    }
		pthread_mutex_unlock(&mutex);
	}

	lo_server_thread_free(st);

	return 0;
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	printf("GOT!\n");
	pthread_mutex_lock(&mutex);
	while(1){
		printf("EMERGENCY STOP!\n");
		setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
		setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
		setspeed("/pfs/ur/0006/pb/3/setSpeed", 0);
		setspeed("/pfs/ur/0006/pb/4/setSpeed", 0);
		sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_position[1]);
		sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, axis_position[2]);
		sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, axis_position[5]);
		sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, axis_position[6]);
		usleep(50000);
	}
}

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	loadstate(argv[0]->i);
	pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	if(argv[0]->i<1 || argv[0]->i > 6)
		return 0;
	char str[30];
	sprintf(str, "/pfs/ur/0006/pb/%d/start", argv[0]->i);
	startplayback(str);
	pthread_mutex_unlock(&mutex);
}
